#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define MAX_POINTS 500
#define max_points 30
#define PI 3.14159265358979323846

// 地球半径（单位：米）
const double R = 6371000.0;

// 计算弧度
double deg2rad(double deg) {
    return deg * PI / 180.0;
}

// 中值滤波函数
void medfilt1(double *input, double *output, int n, int window_size) {
    int half_window = window_size / 2;
    for (int i = 0; i < n; i++) {
        int start = (i - half_window) > 0 ? (i - half_window) : 0;
        int end = (i + half_window) < n ? (i + half_window) : n - 1;

        // 提取窗口内的数据
        double window[MAX_POINTS];
        int window_count = 0;
        for (int j = start; j <= end; j++) {
            window[window_count++] = input[j];
        }

        // 对窗口内的数据进行排序
        for (int j = 0; j < window_count - 1; j++) {
            for (int k = j + 1; k < window_count; k++) {
                if (window[j] > window[k]) {
                    double temp = window[j];
                    window[j] = window[k];
                    window[k] = temp;
                }
            }
        }

        // 取中值
        output[i] = window[window_count / 2];
    }
}

// 样条插值函数（简化实现）
void spline_interpolation(double *x, double *y, int n, double *t, double *result, int m) {
    for (int i = 0; i < m; i++) {
        double t_val = t[i];
        int idx = 0;
        while (idx < n - 1 && x[idx + 1] < t_val) {
            idx++;
        }
        double x0 = x[idx];
        double x1 = x[idx + 1];
        double y0 = y[idx];
        double y1 = y[idx + 1];
        result[i] = y0 + (y1 - y0) * (t_val - x0) / (x1 - x0);
    }
}

struct ArrayWrapper {
    double data_xy[max_points][2];
};

struct ArrayWrapper Path_fit(double data[MAX_POINTS][2])
 {
    int n = 500; // 实际数据点数

    // 删除 {0, 0} 的数据
    double new_data[MAX_POINTS][2];
    int new_count = 0;
    for (int i = 0; i < n; i++) {
        if (data[i][0] != 0 || data[i][1] != 0) {
            new_data[new_count][0] = data[i][0];
            new_data[new_count][1] = data[i][1];
            new_count++;
        }
    }

    // 提取经纬度数据
    double longitude[MAX_POINTS], latitude[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        longitude[i] = new_data[i][0];
        latitude[i] = new_data[i][1];
    }

    // 计算与第一个点的经纬度差值
    double delta_lon[MAX_POINTS], delta_lat[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        delta_lon[i] = longitude[i] - longitude[0];
        delta_lat[i] = latitude[i] - latitude[0];
    }

    // 转换为米单位坐标
    double delta_x[MAX_POINTS], delta_y[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        delta_x[i] = delta_lon[i] * (PI / 180.0) * R * cos(deg2rad(latitude[i]));
        delta_y[i] = delta_lat[i] * (PI / 180.0) * R;
    }

    // 中值滤波
    int window_size = 7;
    double x_filtered[MAX_POINTS], y_filtered[MAX_POINTS];
    medfilt1(delta_x, x_filtered, new_count, window_size);
    medfilt1(delta_y, y_filtered, new_count, window_size);

    // 删除离群值
    double residual_x[MAX_POINTS], residual_y[MAX_POINTS];
    for (int i = 0; i < new_count; i++) {
        residual_x[i] = delta_x[i] - x_filtered[i];
        residual_y[i] = delta_y[i] - y_filtered[i];
    }
    double threshold_x = 0.1, threshold_y = 0.1; // 阈值
    int outliers[MAX_POINTS] = {0};
    for (int i = 0; i < new_count; i++) {
        if (fabs(residual_x[i]) > threshold_x || fabs(residual_y[i]) > threshold_y) {
            outliers[i] = 1;
        }
    }
    double x_cleaned[MAX_POINTS], y_cleaned[MAX_POINTS];
    int cleaned_count = 0;
    for (int i = 0; i < new_count; i++) {
        if (!outliers[i]) {
            x_cleaned[cleaned_count] = x_filtered[i];
            y_cleaned[cleaned_count] = y_filtered[i];
            cleaned_count++;
        }
    }

    // 参数化数据点：计算累积距离
    double dist[MAX_POINTS] = {0};
    for (int i = 1; i < cleaned_count; i++) {
        dist[i] = sqrt(pow(x_cleaned[i] - x_cleaned[i - 1], 2) + pow(y_cleaned[i] - y_cleaned[i - 1], 2));
    }
    double cumdist[MAX_POINTS] = {0};
    for (int i = 1; i < cleaned_count; i++) {
        cumdist[i] = cumdist[i - 1] + dist[i];
    }

    // 生成均匀分布的参数值
    int num_points = 10000;
    double t_uniform[num_points];
    for (int i = 0; i < num_points; i++) {
        t_uniform[i] = (cumdist[cleaned_count - 1] * i) / (num_points - 1);
    }

    // 样条插值拟合
    double x_fit[num_points], y_fit[num_points];
    spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform, x_fit, num_points);
    spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform, y_fit, num_points);

    // 均匀采样30个点
    int num_uniform = 30;
    double t_uniform_samples[num_uniform];
    for (int i = 0; i < num_uniform; i++) {
        t_uniform_samples[i] = (cumdist[cleaned_count - 1] * i) / (num_uniform - 1);
    }
    double x_uniform[num_uniform], y_uniform[num_uniform];
    spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform_samples, x_uniform, num_uniform);
    spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform_samples, y_uniform, num_uniform);

    struct ArrayWrapper xy;
    // 输出均匀采样的点
    for (int i = 0; i < num_uniform; i++) {
        xy.data_xy[i][0] = x_uniform[i];
        xy.data_xy[i][1] = y_uniform[i];
    }

    return xy;
}